#include "lavida.h"
#include "stdcan.h"

const UdpPidMap mLavidaPidMap[ENG_DATA_SIZE] =
{
    { ENG_DATA_RPM,           0x7e0, 0x22, 0xf4, 0x0c, 0x02, SUPPORTED },
    { ENG_DATA_VS,            0x7e0, 0x22, 0xf4, 0x0d, 0x01, SUPPORTED },
    { ENG_DATA_ECT,           0x7e0, 0x22, 0xf4, 0x05, 0x01, SUPPORTED },
    { ENG_DATA_IAT,           0x7e0, 0x22, 0xf4, 0x0f, 0x01, SUPPORTED },
    { ENG_DATA_APP,           0x7e0, 0x22, 0x10, 0x26, 0x02, SUPPORTED },
    { ENG_DATA_TP,            0x7e0, 0x22, 0xf4, 0x43, 0x02, SUPPORTED },
    { ENG_DATA_ERT,           0x7e0, 0x22, 0x38, 0x87, 0x04, SUPPORTED },
    { ENG_DATA_LOAD,          0x7e0, 0x22, 0x10, 0x1c, 0x02, SUPPORTED },
    { ENG_DATA_BATTVOLT,      0x7e0, 0x00, 0x00, 0x00, 0x00, UNSUPPORTED },
    { ENG_DATA_REALFUELCO,    0x7e0, 0x22, 0x11, 0x3f, 0x02, SUPPORTED },
    { ENG_DATA_FUELLEVEL,     0x7e0, 0x22, 0x14, 0xb4, 0x02, SUPPORTED },
    { ENG_DATA_ODO,           0x7e0, 0x22, 0x10, 0x60, 0x04, SUPPORTED },
    { ENG_DATA_LTFT,          0x7e0, 0x22, 0xf4, 0x07, 0x01, SUPPORTED },
    { ENG_DATA_STFT,          0x7e0, 0x22, 0xf4, 0x06, 0x01, SUPPORTED },
    { ENG_DATA_MISFIRE1,      0x7e0, 0x22, 0x11, 0x4f, 0x02, SUPPORTED },
    { ENG_DATA_MISFIRE2,      0x7e0, 0x22, 0x11, 0x52, 0x02, SUPPORTED },
    { ENG_DATA_MISFIRE3,      0x7e0, 0x22, 0x11, 0x50, 0x02, SUPPORTED },
    { ENG_DATA_MISFIRE4,      0x7e0, 0x22, 0x11, 0x51, 0x02, SUPPORTED },
    { ENG_DATA_MISFIRE5,      0x7e0, 0x00, 0x00, 0x00, 0x00, UNSUPPORTED },
    { ENG_DATA_MISFIRE6,      0x7e0, 0x00, 0x00, 0x00, 0x00, UNSUPPORTED },
    { ENG_DATA_FCLS,          0x7e0, 0x00, 0x00, 0x00, 0x00, UNSUPPORTED },
    { ENG_DATA_KEYSTATUS,     0x7e0, 0x00, 0x00, 0x00, 0x00, UNSUPPORTED },
    { ENG_DATA_HO2S1,         0x7e0, 0x22, 0xf4, 0x14, 0x01, SUPPORTED },
    { ENG_DATA_HO2S2,         0x7e0, 0x22, 0xf4, 0x14, 0x01, SUPPORTED },
    { ENG_DATA_MAP,           0x7e0, 0x22, 0xf4, 0x0b, 0x01, SUPPORTED },
    { ENG_DATA_INJECTPULSE,   0x7e0, 0x00, 0x00, 0x00, 0x00, UNSUPPORTED },
    { ENG_DATA_OILPRESSURE,   0x7e0, 0x00, 0x00, 0x00, 0x00, UNSUPPORTED },
    { ENG_DATA_OILLEVELSTATUS,0x7e0, 0x00, 0x00, 0x00, 0x00, UNSUPPORTED },
};

const UdpPidMap mLavidaBcmPidMap[BCM_DATA_SIZE] =
{
    { BCM_DATA_LFWIN,          0x74a, 0x22, 0x18, 0x42, 0x01, SUPPORTED },
    { BCM_DATA_RFWIN,          0x74b, 0x22, 0x18, 0x52, 0x01, SUPPORTED },
    { BCM_DATA_LRWIN,          0x74a, 0x22, 0x19, 0x42, 0x01, SUPPORTED },
    { BCM_DATA_RRWIN,          0x74b, 0x22, 0x19, 0x52, 0x01, SUPPORTED },
    { BCM_DATA_DDA,            0x74a, 0x22, 0x19, 0x44, 0x01, SUPPORTED },
    { BCM_DATA_PDA,            0x74b, 0x22, 0x19, 0x48, 0x01, SUPPORTED },
    { BCM_DATA_RRDA,           0x70e, 0x22, 0x19, 0x50, 0x01, SUPPORTED },
    { BCM_DATA_LRDA,           0x70e, 0x22, 0x19, 0x4c, 0x01, SUPPORTED },
    { BCM_DATA_SUNROOF,        0x70e, 0x22, 0x19, 0x81, 0x01, SUPPORTED },
    { BCM_DATA_PARKLAMP,       0x74a, 0x22, 0x19, 0x43, 0x01, SUPPORTED },
    { BCM_DATA_PARKLAMP,       0x000, 0x22, 0x19, 0x43, 0x01, UNSUPPORTED },
    { BCM_DATA_CHARGESTATUS,   0x710, 0x22, 0x2a, 0x0f, 0x01, UNSUPPORTED },
    { BCM_DATA_BATTCURRENT,    0x710, 0x22, 0x2a, 0x09, 0x04, SUPPORTED },
    { BCM_DATA_BATTSTATUS,     0x000, 0x22, 0x2a, 0x09, 0x01, SUPPORTED },
    { BCM_DATA_BATTVOLT,       0x710, 0x22, 0x2a, 0x07, 0x02, SUPPORTED },
    { BCM_DATA_ODO,            0x710, 0x22, 0x2a, 0x07, 0x02, UNSUPPORTED },
};

VehicleCtrlOps mLavidaCtrlOps;
VehicleProcessOps mLavidaProcessOps;
uint8_t mLavidaEngineOn = TRUE;

void lavida_init(Vehicle *vehicle)
{
    logi("%s", __func__);
    mLavidaProcessOps.check_engine_on = lavida_check_engine_on;
    mLavidaProcessOps.process_eng_data = lavida_process_eng_data;
    //mLavidaProcessOps.process_bcm_data = lavida_process_bcm_data;

    vehicle->ctrlOps = &mLavidaCtrlOps;
    vehicle->processOps = &mLavidaProcessOps;

    bsp_can_filter(0x7e8, 0x7e8, 0x7ff, 0x7ff);
}

uint8_t lavida_check_engine_on(void)
{
    return mLavidaEngineOn;
}

void lavida_process_eng_data(EngineDataMap *dataMap)
{

    uint8_t i;
    uint8_t support;
    uint8_t size;
    CanTxMsg txMsg =
    {
        0x7e0, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x03, 0x00, 0x00, 0x00, 0x55, 0x55, 0x55, 0x55
    };
    CanRxMsg rxMsg;

    logi("%s", __func__);
    for(i = 0; i < ENG_DATA_SIZE; i++) {
        os_delay(2);
        vehicle_lock();
        support = mLavidaPidMap[i].support;
        //check wether this id is supported?
        if(support == UNSUPPORTED) {
            vehicle_unlock();
            continue;
        }
        size = mLavidaPidMap[i].size;

        bsp_can_clear();
        bsp_can_filter(0x7e8, 0x7e8, 0x7ff, 0x7ff);
        txMsg.Data[0] = 0x03;
        txMsg.Data[1] = mLavidaPidMap[i].cmd;
        txMsg.Data[2] = mLavidaPidMap[i].pids[0];
        txMsg.Data[3] = mLavidaPidMap[i].pids[1];
        if(vehicle_can_session(&txMsg, &rxMsg)) {
            if(rxMsg.Data[1] == (txMsg.Data[1] + 0x40)
                    || rxMsg.Data[2] == txMsg.Data[2]
                    || rxMsg.Data[3] == txMsg.Data[3])
            dataMap->data[i].id = i;
            dataMap->data[i].updated = TRUE;
            dataMap->data[i].len = size;
            memcpy(dataMap->data[i].data, rxMsg.Data + 4, size);
            if(i == ENG_DATA_RPM) {
                mLavidaEngineOn = TRUE;
            }
        } else {
            if(i == ENG_DATA_RPM) {
                mLavidaEngineOn = FALSE;
            }
            vehicle_unlock();
            continue;
        }
        vehicle_unlock();
    }
}

void lavida_process_bcm_data(BcmDataMap *dataMap)
{
    uint8_t i;
    uint8_t support;
    uint8_t size;
    uint8_t offset;
    uint16_t txid, rxid;
    CanTxMsg txMsg =
    {
        0x7e0, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x03, 0x00, 0x00, 0x00, 0x55, 0x55, 0x55, 0x55
    };
    CanRxMsg rxMsg;

    logi("%s", __func__);
    for(i = 0; i < BCM_DATA_SIZE; i++) {
        os_delay(2);
        support = mLavidaBcmPidMap[i].support;
        //check wether this id is supported?
        if(support == UNSUPPORTED) {
            continue;
        }
        size = mLavidaBcmPidMap[i].size;
        txid = mLavidaBcmPidMap[i].txid;
        if(txid == 0x74a)
            rxid = 0x7b4;
        if(txid == 0x74b)
            rxid = 0x7b5;
        if(txid == 0x70e)
            rxid = 0x778;
        if(txid == 0x710)
            rxid = 0x77a;

        bsp_can_clear();
        bsp_can_filter(rxid, rxid, rxid | 0x0ff, rxid | 0x0ff);
        txMsg.StdId = txid;
        txMsg.Data[0] = 0x03;
        txMsg.Data[1] = mLavidaBcmPidMap[i].cmd;
        txMsg.Data[2] = mLavidaBcmPidMap[i].pids[0];
        txMsg.Data[3] = mLavidaBcmPidMap[i].pids[1];
        if(vehicle_can_session(&txMsg, &rxMsg)) {
            if(rxMsg.Data[1] == (txMsg.Data[1] + 0x40)
                    || rxMsg.Data[2] == txMsg.Data[2]
                    || rxMsg.Data[3] == txMsg.Data[3])
            dataMap->data[i].id = i;
            dataMap->data[i].updated = TRUE;
            dataMap->data[i].len = size;
            memcpy(dataMap->data[i].data, rxMsg.Data + 4, size);
        } else {
            continue;
        }
    }
}
